/** @file demo_flight_control.cpp
 *  @version 3.3
 *  @date September, 2017
 *
 *  @brief
 *  demo sample of how to use Local position control
 *
 *  @copyright 2017 DJI. All rights reserved.
 *
 */

#include "dji_sdk_demo/demo_local_position_control.h"
#include "dji_sdk/dji_sdk.h"
#include <vector>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <cstdlib>

using namespace std;
ros::ServiceClient set_local_pos_reference;
ros::ServiceClient sdk_ctrl_authority_service;
ros::ServiceClient drone_task_service;
ros::ServiceClient query_version_service;

ros::Publisher ctrlPosYawPub;
ros::Publisher ctrlBrakePub;
ros::Publisher marker_pub;

// 全局订阅者：姿态、速度、aruco 坐标、任务点
ros::Subscriber attitude_sub;
ros::Subscriber velocity_sub;
ros::Subscriber ArUco_sub;
ros::Subscriber mission_point_sub; 

// 全局发布者：里程计、任务点
ros::Publisher odom_pub;
ros::Publisher mission_point_pub;

// 全局velocity quaternion变量
geometry_msgs::Vector3Stamped v;
geometry_msgs::QuaternionStamped q;
geometry_msgs::PoseStamped aruco;

geometry_msgs::PointStamped local_position;
sensor_msgs::NavSatFix current_gps_position;
geometry_msgs::PoseStamped mission;
// global variables for subscribed topics
uint8_t flight_status = 255;
uint8_t display_mode  = 255;
uint8_t current_gps_health = 0;

void generate_2point(vector< vector<double>>& vec);

// 二维向量存储任务点
vector< vector<double >> Point;
// 总任务数量
static int num_targets = 255;
// 当前任务状态
static int now_state=0;
// 接收状态
static bool receive_state = false;
// 起飞结果标志
bool takeoff_result = false;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "demo_local_position_control_node");
  ros::NodeHandle nh;

  // 直接生成任务点
  generate_2point(Point);
  // 发布：里程计、任务点
  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
  mission_point_pub = nh.advertise<geometry_msgs::PoseStamped>("mission_point", 10);
  
  // 订阅：（姿态、速度）用来发布 odom 信息、（aruco 信息）用来执行飞行任务、任务点
  attitude_sub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);
  velocity_sub = nh.subscribe("dji_sdk/velocity", 10, &velocity_callback);
  ArUco_sub = nh.subscribe("/aruco_single/pose", 10, &ArUco_callback);
  mission_point_sub = nh.subscribe("mission_point", 10, &mission_point_callback);
 
  // 订阅： 飞行状态信息、显示无人机状态信息、本地 ENU 坐标、GPS 坐标、GPS 健康度 
  ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback);
  ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);
  ros::Subscriber localPosition = nh.subscribe("dji_sdk/local_position", 10, &local_position_callback);
  ros::Subscriber gpsSub      = nh.subscribe("dji_sdk/gps_position", 10, &gps_position_callback);
  ros::Subscriber gpsHealth      = nh.subscribe("dji_sdk/gps_health", 10, &gps_health_callback);

  // 发布：位置控制
  ctrlPosYawPub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
  ctrlBrakePub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_generic", 10);
 
  // 服务：SDK控制权、飞行状态（起飞降落回家）、查询飞机版本、设置参考点
  sdk_ctrl_authority_service = nh.serviceClient<dji_sdk::SDKControlAuthority> ("dji_sdk/sdk_control_authority");
  drone_task_service         = nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
  query_version_service      = nh.serviceClient<dji_sdk::QueryDroneVersion>("dji_sdk/query_drone_version");
  set_local_pos_reference    = nh.serviceClient<dji_sdk::SetLocalPosRef> ("dji_sdk/set_local_pos_ref");

  // 获取飞机控制权
  bool obtain_control_result = obtain_control();

  if (!set_local_position())
  {
    ROS_ERROR("GPS health insufficient - No local frame reference for height. Exiting.");
    return 1;
  }

  if(is_M100())
  {
    ROS_INFO("M100 taking off!");
	while(!takeoff_result )
	{
    takeoff_result = M100monitoredTakeoff();
	}
  }
  else
  {
    ROS_INFO("A3/N3 taking off!");
    takeoff_result = monitoredTakeoff();
  }

  if(takeoff_result)
  {
    ROS_INFO("Successfully taking off!!");
    // 当前任务状态
   	now_state = 1;
    // 任务开始，将任务状态设为 1
    target_set_state = 1;
    // 开始接收任务点
    receive_state = false;

}
  ros::spin();
  return 0;
}

/*!
 * 当本地位置可用时，调用此函数
 */
void local_position_callback(const geometry_msgs::PointStamped::ConstPtr& msg) {
  // 获取开始时间
  static ros::Time start_time = ros::Time::now();
  // 获取时间差
  ros::Duration elapsed_time = ros::Time::now() - start_time;
  
  local_position = *msg;
  
  double xCmd, yCmd, zCmd;
  
  sensor_msgs::Joy controlPosYaw;

  // 50 Hz执行频率
  if (elapsed_time > ros::Duration(0.02)) {
    start_time = ros::Time::now();
    // 定义一个 odom 的消息对象

    nav_msgs::Odometry odom;
    // 时间戳
    odom.header.stamp = ros::Time::now();

    // 父坐标系
    odom.header.frame_id = "odom";

    // odom 下的坐标
    odom.pose.pose.position.x = local_position.point.x;
    odom.pose.pose.position.y = local_position.point.y;
    odom.pose.pose.position.z = local_position.point.z;
    odom.pose.pose.orientation = q.quaternion;

    // base_link 下的速度
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = v.vector.x;
    odom.twist.twist.linear.y = v.vector.y;
    odom.twist.twist.linear.z = v.vector.z;

    // 发布 odom 里程计消息
    odom_pub.publish(odom);
	if(takeoff_result)
{
    if (target_set_state == now_state) {
      // 如果有至少三颗卫星
      if (current_gps_health > 3) 
	  {
        // 如果任务全部执行完成，则飞机降落
          if (!Point.size()) {
              Landing();
          }
            
        // 否则继续执行任务
          else 
             { setTarget(Point[0][0], Point[0][1], Point[0][2], Point[0][3]);
               local_position_ctrl(xCmd, yCmd, zCmd);
			}
      }

      else
      {
        ROS_INFO("Cannot execute Local Position Control");
        ROS_INFO("Not enough GPS Satellites");
        target_set_state = 0;
      }
	 }
}
    }
  }


/*!
 * 位置指令发布函数
 */
void local_position_ctrl(double &xCmd, double &yCmd, double &zCmd)
{
  // 速度因子	
  float speedFactor = 0.1;
  float limit = 0.05;
  xCmd = target_offset_x - local_position.point.x;
  yCmd = target_offset_y - local_position.point.y;
  zCmd = target_offset_z;

  if (abs(xCmd) >= speedFactor) xCmd = (xCmd > 0) ? speedFactor : -1 * speedFactor;
  if (abs(yCmd) >= speedFactor) yCmd = (yCmd > 0) ? speedFactor : -1 * speedFactor;

  sensor_msgs::Joy controlPosYaw;
  controlPosYaw.axes.push_back(xCmd);
  controlPosYaw.axes.push_back(yCmd);
  controlPosYaw.axes.push_back(zCmd);
  controlPosYaw.axes.push_back(target_yaw);
  ctrlPosYawPub.publish(controlPosYaw);

// 四元数转欧拉角
// atan((2 * ( q.quaternion.w * q.quaternion.z + q.quaternion.x * q.quaternion.y )) / (1 - 2 * (q.quaternion.z * q.quaternion.z + q.quaternion.y * q.quaternion.y))) * 180 / 3.141592654

  ROS_INFO("Delta:--------xCmd: %f, yCmd: %f, zCmd: %f", xCmd,yCmd,zCmd);
  ROS_INFO("Now_positon:--X:%f, Y:%f, Z:%f",local_position.point.x,local_position.point.y,local_position.point.z);

  if (((std::abs(xCmd)) < limit) && ((std::abs(yCmd)) < limit) &&
      (local_position.point.z > (target_offset_z - 0.1)) && (local_position.point.z < (target_offset_z + 0.1))) {
    if(target_set_state <= num_targets) {
      	ROS_INFO("%d of %d target(s) complete", target_set_state, num_targets);
     	ROS_INFO("Mission %d finished!!!!!!!!!!!!!!!!!!!!!!!!!", target_set_state);
      	target_set_state++;
		now_state++;
        // 删除本次任务点
		Point.erase(Point.begin());
        // 开始接收新的任务点
        //receive_state = true;
     
    }
    else
    {
      target_set_state = 0;
      ROS_INFO("Mission finished!");
    }
  }
}

bool takeoff_land(int task)
{
  dji_sdk::DroneTaskControl droneTaskControl;

  droneTaskControl.request.task = task;

  drone_task_service.call(droneTaskControl);

  if(!droneTaskControl.response.result)
  {
    ROS_ERROR("takeoff_land fail");
    return false;
  }

  return true;
}

bool obtain_control()
{
  dji_sdk::SDKControlAuthority authority;
  authority.request.control_enable=1;
  sdk_ctrl_authority_service.call(authority);

  if(!authority.response.result)
  {
    ROS_ERROR("obtain control failed!");
    return false;
  }

  return true;
}

bool is_M100()
{
  dji_sdk::QueryDroneVersion query;
  query_version_service.call(query);

  if(query.response.version == DJISDK::DroneFirmwareVersion::M100_31)
  {
    return true;
  }

  return false;
}


void gps_position_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
  
  // 持续发送任务点
  send_mission_point();
  current_gps_position = *msg;
}

void gps_health_callback(const std_msgs::UInt8::ConstPtr& msg) {
  current_gps_health = msg->data;
}

void flight_status_callback(const std_msgs::UInt8::ConstPtr& msg)
{
  flight_status = msg->data;
}

void display_mode_callback(const std_msgs::UInt8::ConstPtr& msg)
{
  display_mode = msg->data;
}


/*!
 * This function demos how to use the flight_status
 * and the more detailed display_mode (only for A3/N3)
 * to monitor the take off process with some error
 * handling. Note M100 flight status is different
 * from A3/N3 flight status.
 */
bool
monitoredTakeoff()
{
  ros::Time start_time = ros::Time::now();

  if(!takeoff_land(dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF)) {
    return false;
  }

  ros::Duration(0.01).sleep();
  ros::spinOnce();

  // Step 1.1: Spin the motor
  while (flight_status != DJISDK::FlightStatus::STATUS_ON_GROUND &&
         display_mode != DJISDK::DisplayMode::MODE_ENGINE_START &&
         ros::Time::now() - start_time < ros::Duration(5)) {
    ros::Duration(0.01).sleep();
    ros::spinOnce();
  }

  if(ros::Time::now() - start_time > ros::Duration(5)) {
    ROS_ERROR("Takeoff failed. Motors are not spinnning.");
    return false;
  }
  else {
    start_time = ros::Time::now();
    ROS_INFO("Motor Spinning ...");
    ros::spinOnce();
  }


  // Step 1.2: Get in to the air
  while (flight_status != DJISDK::FlightStatus::STATUS_IN_AIR &&
         (display_mode != DJISDK::DisplayMode::MODE_ASSISTED_TAKEOFF || display_mode != DJISDK::DisplayMode::MODE_AUTO_TAKEOFF) &&
         ros::Time::now() - start_time < ros::Duration(20)) {
    ros::
Duration(0.01).sleep();
    ros::spinOnce();
  }

  if(ros::Time::now() - start_time > ros::Duration(20)) {
    ROS_ERROR("Takeoff failed. Aircraft is still on the ground, but the motors are spinning.");
    return false;
  }
  else {
    start_time = ros::Time::now();
    ROS_INFO("Ascending...");
    ros::spinOnce();
  }

  // Final check: Finished takeoff
  while ( (display_mode == DJISDK::DisplayMode::MODE_ASSISTED_TAKEOFF || display_mode == DJISDK::DisplayMode::MODE_AUTO_TAKEOFF) &&
          ros::Time::now() - start_time < ros::Duration(20)) {
    ros::Duration(0.01).sleep();
    ros::spinOnce();
  }

  if ( display_mode != DJISDK::DisplayMode::MODE_P_GPS || display_mode != DJISDK::DisplayMode::MODE_ATTITUDE)
  {
    ROS_INFO("Successful takeoff!");
    start_time = ros::Time::now();
  }
  else
  {
    ROS_ERROR("Takeoff finished, but the aircraft is in an unexpected mode. Please connect DJI GO.");
    return false;
  }

  return true;
}


/*!
 * This function demos how to use M100 flight_status
 * to monitor the take off process with some error
 * handling. Note M100 flight status is different
 * from A3/N3 flight status.
 */
bool
M100monitoredTakeoff()
{
  ros::Time start_time = ros::Time::now();

  float home_altitude = current_gps_position.altitude;

  // 发送飞机起飞请求
  if(!takeoff_land(dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF))
  {
    return false;
  }

  // 延时 10ms
  ros::Duration(0.01).sleep();
  ros::spinOnce();

  // Step 1: If M100 is not in the air after 10 seconds, fail.
  while (ros::Time::now() - start_time < ros::Duration(15))
  {
    ros::Duration(0.01).sleep();
    ros::spinOnce();
  }

ROS_INFO("waiting...................");
ROS_INFO("waiting...................");
ROS_INFO("waiting...................");
ROS_INFO("waiting...................");
ROS_INFO("waiting...................");
  if(flight_status != DJISDK::M100FlightStatus::M100_STATUS_IN_AIR ||
     current_gps_position.altitude - home_altitude < 1.0)
  {
    ROS_ERROR("Takeoff failed.");
    return false;
  }
  else
  {
    start_time = ros::Time::now();
    ROS_INFO("Successful takeoff!");
    ros::spinOnce();
  }
  return true;
}

bool set_local_position()
{
  dji_sdk::SetLocalPosRef localPosReferenceSetter;
  set_local_pos_reference.call(localPosReferenceSetter);

  return (bool)localPosReferenceSetter.response.result;
}

// 飞机降落
void Landing()
{
    dji_sdk::DroneTaskControl droneTaskControl;

    droneTaskControl.request.task = 6;

    drone_task_service.call(droneTaskControl);

    if (droneTaskControl.response.result)
    {
        ROS_INFO("Successful Landing");
    }
    //if (!droneTaskControl.response.result)
    //{
    //    ROS_ERROR("Landing fail");
    //    return false;
    //}
    //return true;
    
}


// 人工产生任务点
void generate_2point(vector< vector<double>>& vec)
{
    /* 步长 */
    double step = 90;

    /* 半径 */
    double r = 0.6;

    /* 角度值 */
    static double theta = 0;

    // 插入元素
    for (int i = 0; i < 6; i++)
    {
        // 回到出发点
        if (i == 5)
        {
            vec.push_back(vector<double >());
            vec[i].push_back(0);
            vec[i].push_back(0);
            vec[i].push_back(1.5);
            vec[i].push_back(90);
            cout << "origin point:" << endl;
        }
        else
        {
            vec.push_back(vector<double >());
            vec[i].push_back(r * cos(theta * 3.1415926 / 180));
            vec[i].push_back(r * sin(theta * 3.1415926 / 180));
            vec[i].push_back(1.5);
            vec[i].push_back(((45 + theta) * 3.141592654 / 180));
            cout << "theta:" << theta << endl;
            theta += step;
        }
        cout << "第 %d 步" << i << endl;
    }

}

void velocity_callback(const geometry_msgs::Vector3Stamped::ConstPtr& msg) {
    v = *msg;
}
void attitude_callback(const geometry_msgs::QuaternionStamped::ConstPtr& msg) {
    q = *msg;
}

// ArUco 回调函数
void ArUco_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    // 获取初始时间
    static ros::Time start_time = ros::Time::now();
    // 计算时间差
    ros::Duration elapsed_time = ros::Time::now() - start_time;
    // 得到 aruco 数据
    aruco = *msg;
    double xCmd, yCmd, zCmd;
    sensor_msgs::Joy controlPosYaw;

    // 执行频率为 50Hz
	if (elapsed_time > ros::Duration(0.02)) {
        // 更新初始时间
        start_time = ros::Time::now();
		
		if(takeoff_result)
		{
        // 判断任务是否全部执行完成
        if (!Point.size()) {
            // 飞机降落
            Landing();
        }
        
        else
        {   // 执行任务点
            if (target_set_state == now_state)
            {
                setTarget(Point[0][0], Point[0][1], Point[0][2], Point[0][3]);
                aruco_position_ctrl(xCmd, yCmd, zCmd);
            }
        }
		}
  }
}

// 在 ArUco 坐标下的位置控制
void aruco_position_ctrl(double &xCmd, double &yCmd, double &zCmd)
{
  // 速度因子	
  float speedFactor = 0.1;
  float limit = 0.05;
  // 计算与目标位置的差值
  xCmd = target_offset_x - aruco.pose.position.x;
  yCmd = target_offset_y - aruco.pose.position.y;
  zCmd = target_offset_z;

  // 限速
  if (abs(xCmd) >= speedFactor) xCmd = (xCmd > 0) ? speedFactor : -1 * speedFactor;
  if (abs(yCmd) >= speedFactor) yCmd = (yCmd > 0) ? speedFactor : -1 * speedFactor;

  // 发布控制指令
  sensor_msgs::Joy controlPosYaw;
  controlPosYaw.axes.push_back(xCmd);
  controlPosYaw.axes.push_back(yCmd);
  controlPosYaw.axes.push_back(zCmd);
  controlPosYaw.axes.push_back(target_yaw);
  ctrlPosYawPub.publish(controlPosYaw);

  // 四元数转欧拉角
// atan((2 * ( q.quaternion.w * q.quaternion.z + q.quaternion.x * q.quaternion.y )) / (1 - 2 * (q.quaternion.z * q.quaternion.z + q.quaternion.y * q.quaternion.y))) * 180 / 3.141592654

  ROS_INFO("xCmd: %f, yCmd: %f, zCmd: %f", xCmd,yCmd,zCmd);
  ROS_INFO("Now_position: --------------- x:%f, y:%f, z:%f", aruco.pose.position.x, aruco.pose.position.y, aruco.pose.position.z);

  if (((std::abs(xCmd)) < limit) && ((std::abs(yCmd)) < limit) &&
      (aruco.pose.position.z > (target_offset_z - 0.1)) && (aruco.pose.position.z < (target_offset_z + 0.1))) {
    if(target_set_state <= num_targets) {
      	ROS_INFO("%d of %d target(s) complete", target_set_state, num_targets);
     	ROS_INFO("Mission %d finished!!!!!!!!!!!!!!!!!!!!!!!!!", target_set_state);
      	target_set_state++;
		now_state++;
		Point.erase(Point.begin());
        // 开始接收新的任务点
        // receive_state = true;
    }
    else
    {
      target_set_state = 0;
      ROS_INFO("Mission finished!");
    }
  }
}

// 随机产生任务点，并发送
// 函数在 gps_position_callback（）持续执行，发送一次就停止，等待下一次发送的信号；
void send_mission_point() {
    // 开始发送
    if (receive_state)
    {
        double positionX, positionY, positionZ;
        double orientationX, orientationY, orientationZ, orientationW;
        double roll, pitch, yaw;
        double ang2rad = 3.141592654 / 180;

        // 产生随机任务点
        positionX = getRand(-10, 10);
        positionY = getRand(-10, 10);
        positionZ = 2;
        roll = 0, pitch = 0, yaw = 0;

        // 航向角转四元数
        orientationW = cos(ang2rad * roll / 2) * cos(ang2rad * yaw / 2) * cos(ang2rad * pitch / 2) + sin(ang2rad * roll / 2) * sin(ang2rad * yaw / 2) * sin(ang2rad * pitch / 2);
        orientationX = sin(ang2rad * roll / 2) * cos(ang2rad * yaw / 2) * cos(ang2rad * pitch / 2) - cos(ang2rad * roll / 2) * sin(ang2rad * yaw / 2) * sin(ang2rad * pitch / 2);
        orientationY = cos(ang2rad * roll / 2) * sin(ang2rad * yaw / 2) * cos(ang2rad * pitch / 2) + sin(ang2rad * roll / 2) * cos(ang2rad * yaw / 2) * sin(ang2rad * pitch / 2);
        orientationZ = cos(ang2rad * roll / 2) * cos(ang2rad * yaw / 2) * sin(ang2rad * pitch / 2) - sin(ang2rad * roll / 2) * sin(ang2rad * yaw / 2) * cos(ang2rad * pitch / 2);

        // PoseStamped 消息对象
        geometry_msgs::PoseStamped msg;
        // 时间戳
        msg.header.stamp = ros::Time::now();
        // 位置
        msg.pose.position.x = positionX;
        msg.pose.position.y = positionY;
        msg.pose.position.z = positionZ;
        // 四元数
        msg.pose.orientation.x = orientationX;
        msg.pose.orientation.y = orientationY;
        msg.pose.orientation.z = orientationZ;
        msg.pose.orientation.w = orientationW;

        // 发送任务点
        mission_point_pub.publish(msg);
        // 停止发送
        receive_state = false;
    }
}

// 左闭右闭区间
int getRand(int min, int max) {
    return (rand() % (max - min + 1)) + min;
}

// 任务接收回调函数
void mission_point_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
	
	ROS_INFO("Receive mission_point !!!!!!!!!!!!!!!!!! ");
    // 获取任务点信息
	mission = *msg;
    // 存入任务容器中
    Point.push_back(vector<double >());
    Point[0].push_back( mission.pose.position.x);
    Point[0].push_back( mission.pose.position.y);
    Point[0].push_back( mission.pose.position.z);
    Point[0].push_back( atan((2 * (mission.pose.orientation.w * mission.pose.orientation.z + mission.pose.orientation.x * mission.pose.orientation.y )) / (1 - 2 * (mission.pose.orientation.z * mission.pose.orientation.z + mission.pose.orientation.y * mission.pose.orientation.y))) * 180 / 3.141592654);
	ROS_INFO("X:%f, Y:%f, Z:%f, yaw:%f ",Point[0][0],Point[0][1],Point[0][2],Point[0][3]);
	
}


